snippet rospy "Rospy preamble" !b
#!/usr/bin/env python

PACKAGE = '${1:`!v b:ros_package_name`}'
NODE = '${2:`!v Filename("$1")`}'

import rospy

$0
endsnippet

snippet init "Initialize node" !b
rospy.init_node(NODE${1:, anonymous=True})$0
endsnippet

snippet "acli(ent)?" "Action client" !br
${1:client} = ${2:actionlib.}SimpleActionClient('${3:service}', ${4:msg.MyAction})$0
endsnippet

snippet "log(\w?)" "Log message" br
rospy.log`!p
def get_level(c):
  levels = ["debug", "info", "warn", "err", "fatal"]
  for l in levels:
    if l.startswith(c.lower()):
      return l
  return "info"
snip.rv = get_level(match.group(1) or 'i')
`('${1:Message}'${2/.+/ % (/}${2}${2/.+/)/})$0
endsnippet

snippet state "Smach state class" !b
class ${1:StateName}(smach.State):
	def __init__(self):
		smach.State.__init__(self,
			outcomes=[${2:'outcome1'}],
			input_keys=[${3:'input'}],
			output_keys=[${4:'output'}])
	def execute(self, userdata):
		pass
endsnippet

snippet par "Query parameter from server" !b
${1/[\/~]?(.+)/$1/} = rospy.get_param('${1:param_name}'${2/.+/, /}${2:default}${2/.+//})$0
endsnippet


###########################################################################
#                          Publisher/Subscriber                           #
###########################################################################

snippet pub "Publisher" !b
${1:publisher} = rospy.Publisher('${2:topic}', ${3:message_type}, queue_size=${4:10}${5/.+/, latch=/}${5:True}${5/.+//})$0
endsnippet

snippet sub "Subscriber" !b
${1:subscriber} = rospy.Subscriber('${2:topic}', ${3:message_type}, ${4:callback})$0
endsnippet


###########################################################################
#                                Services                                 #
###########################################################################

snippet "scli(ent)?" "Service client" !br
${1:client} = rospy.ServiceProxy('${2:service}', ${3:srv.Type})$0
endsnippet

snippet "s(srv|server)" "Service server" !br
${1:server} = rospy.Service('${2:service}', ${3:srv.Type}, ${4:callback})$0
endsnippet

snippet scall "Service call" !b
try:
	${1:response} = ${2:some_srv}(${3:request})
except rospy.ServiceException as e:
	print "Service call failed: %s" % str(e)
endsnippet


###########################################################################
#                             Transformations                             #
###########################################################################

snippet e2q "Euler to quaternion"
${1:q} = ${2:tf.transformations.}quaternion_from_euler(${3:0, 0, yaw})$0
endsnippet

snippet e2qm "Euler to quaternion message"
${1:msg} = Quaternion(*${2:tf.transformations.}quaternion_from_euler(${3:0, 0, yaw}))$0
endsnippet

snippet y2q "Yaw to quaternion"
${1:q} = ${2:tf.transformations.}quaternion_from_euler(0, 0, ${3:yaw})$0
endsnippet

snippet q2e "Quaternion to euler"
${1:rpy} = ${2:tf.transformations.}euler_from_quaternion(${3:q})$0
endsnippet

snippet qm2e "Quaternion message to euler"
o = ${1:msg.pose.orientation}
${2:rpy} = ${3:tf.transformations.}euler_from_quaternion([o.x, o.y, o.z, o.w])$0
endsnippet

snippet tfl "Transform lookup" !b
import tf
$1 = tf.TransformListener()
try:
	(trans, rot) = ${1:tf_listener}.lookupTransform(${2:'/odom'}, ${3:'/base_link'}, rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
	continue
endsnippet

###########################################################################
#                                  Time                                   #
###########################################################################

snippet now "Current time"
rospy.Time.now()
endsnippet

snippet rate "Rate" !b
${2:r} = rospy.Rate(${1:hz})$0
endsnippet

snippet timer "Timer"
def $2(event):
	${3:pass}

rospy.Timer(rospy.Duration(${1:s}), ${2:timer_cb})
endsnippet
